

#include "slep.h"
#include "hr_time.h"
#include "epph.h"

FUNVAL nnLeastC(double* A, double* y, double z, OPTS opt){
	int i, iterStep, m, n, steps;
	double t,tp, rsL2, x_norm, x_2norm2, gamma, alpha, norm_xxp, norm_xp, fValp, fVal, r_sum, l_sum, lambda0, root;
	FUNVAL funret;
	double *x, *Ax, *xp, *xxp, *ATy, *s, *v, *g, *vv, *Axy, *funVal, *ValueL, *Av, *As, *xx, *Axp, *ATAs;

	m = opt.m;
	n = opt.n;

	ATy = (double *) malloc( sizeof(double) * n);
	x = (double *) malloc( sizeof(double) * n);
	Ax = (double *) malloc( sizeof(double) * m);
	xp = (double *) malloc( sizeof(double) * n);
	Axp = (double *) malloc( sizeof(double) * m);
	xxp = (double *) malloc( sizeof(double) * n);
	s = (double *) malloc( sizeof(double) * n);
	As = (double *) malloc( sizeof(double) * m);
	ATAs = (double *) malloc( sizeof(double) * n);
	g = (double *) malloc( sizeof(double) * n);
	v = (double *) malloc( sizeof(double) * n);
	vv = (double *) malloc( sizeof(double) * n);
	xx = (double *) malloc( sizeof(double) * n);
	Av = (double *) malloc( sizeof(double) * m);
	Axy = (double *) malloc( sizeof(double) * m);
	funVal = (double *) malloc( sizeof(double) * opt.maxIter);
	ValueL = (double *) malloc( sizeof(double) * opt.maxIter);

	normalizedmv('T',&opt,A,y, ATy);

	rsL2 = opt.rsL2;

	if( opt.init == 2)
		memset(x,0,sizeof(double)*n);
	else{
		if(hasGot(&opt,"x0"))
			dcopy(n,opt.x0,1,x,1);
		else
			for(i =0; i<n; i++)
				x[i]= fabs(ATy[i]);
	}

	normalizedmv('N',&opt,A,x,Ax);

	if( opt.init == 0){
		x_norm = dasum(n,x,1);
		x_2norm2 = ddot(n,x,1,x,1);
		if( x_norm >= 1.0e-6){
			// do some thing here
		}
	}

	gamma = 1.0 + rsL2;

	// xp = x; Axp = Ax; xxp = zeros;
	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);

	tp = 0.0;
	t = 1.0;

	lambda0 = 0.0;

	for( iterStep = 0 ; iterStep < opt.maxIter ; iterStep++ ){
		alpha = (tp -1.0)/t;
		// s = x + alpha * xxp;
		dcopy(n,x,1,s,1);
		daxpy(n,alpha,xxp,1,s,1);

		// As = Ax + alpha * (Ax - Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + alpha, As,1);
		daxpy(m,-alpha,Axp,1,As,1);

		normalizedmv('T',&opt,A,As,ATAs);

		// g = ATAs - ATy + rsL2*s;
		dcopy(n,ATAs,1,g,1);
		daxpy(n,-1.0,ATy,1,g,1);
		daxpy(n,rsL2,s,1,g,1);

		// xp = x; Axp = Ax;
		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);

		for(;;){
			// v = s - g/gamma;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/gamma,g,1,v,1);

			for(i=0; i<n; i++)
				vv[i] = maxof2(v[i],0);
			eplb(xx,&root,&steps,vv,n,z,lambda0);
			lambda0 = root;
			memset(x,0,sizeof(double)*n);
			for(i=0; i<n ; i++)
				if(vv[i]>=0)
					x[i] = xx[i];

			// v = x - s;
			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);

			normalizedmv('N',&opt,A,x,Ax);

			// Av = Ax - As;
			dcopy(m,Ax,1,Av,1);
			daxpy(m,-1.0,As,1,Av,1);

			r_sum = ddot(n,v,1,v,1);
			l_sum = ddot(m,Av,1,Av,1);

			if( l_sum <= r_sum * (gamma - rsL2))
				break;
			else
				gamma = maxof2( 2*gamma, l_sum/r_sum + rsL2);
		}

		tp = t;
		t  = (1.0 + sqrt(4.0 * t * t + 1))/2.0;

		// xxp = x - xp; Axy = Ax - y;
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);
		dcopy(m,Ax,1,Axy,1);
		daxpy(m,-1.0,y,1,Axy,1);
		fValp = fVal = funVal[iterStep] = ddot(m,Axy,1,Axy,1)/2.0 + ddot(n,x,1,x,1)*rsL2/2.0;
		if(iterStep != 0)
			fValp = funVal[iterStep-1];
		norm_xp = dnrm2(n,xp,1);
		norm_xxp = dnrm2(n,xxp,1);

		if(terminationCondition(&opt,fVal,fValp,x,norm_xxp,norm_xp,iterStep))
			break;
	}

	funret.errorCode = 0;
	funret.funVal = funVal;
	funret.totIter = iterStep;
	strcpy(funret.type,"nnLeastR");
	funret.x = x;
	funret.ValueL = ValueL;
	return funret;
}